#ifndef _DISTRIBUTE_OPTIMIZER_H
#define _DISTRIBUTE_OPTIMIZER_H

#include <ewok/ed_ckecking_ring_buffer.h>
#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Point32.h>
#include <std_msgs/Float64MultiArray.h>
#include <sensor_msgs/PointCloud2.h>
#include <visualization_msgs/Marker.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <iostream>
#include <string>
#include <cmath>
#include <stdlib.h>
#include <mavros_msgs/State.h> 
#include <nav_msgs/Odometry.h>
#include <control_msgs/JointControllerState.h>
#include <std_msgs/Float64.h>
#include <queue>
#include <cstdint>
#include <trajectory_msgs/JointTrajectory.h>
#include <std_msgs/ColorRGBA.h>
#include <visualization_msgs/MarkerArray.h>
#include <pcl/filters/passthrough.h>
#include <nav_msgs/Path.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include  <algorithm>
#include "formation_avoidance/DistributedOptimize.h"

#define PARTICLE_NUM 15

const int POW2 = 7;	

extern ewok::EuclideanDistanceCheckingRingBuffer<POW2> rrb;

class DistributeOptimizer
{
public:
	DistributeOptimizer(int id):OptimizerID(id),loop_rate(400),OptimizerColorID(id+32){}
	~DistributeOptimizer(){}

	bool getGradient();
	bool sendVariableAndReceiving();
	bool sendGradient();

	bool receivingVariable();
	bool receivingGradient();

	bool updateVariable();
	void initial_variable(Eigen::Vector4f init_variable);
 	void RegisterCallBack(std::function<double(Eigen::Vector4f,bool,double)> f)
	{
		costJ = f;
	}
	void gradientDescent();
	void init(ros::NodeHandle &nh);
	void optimize_sever_cb(const formation_avoidance::DistributedOptimize::ConstPtr& msg);
	void variable_mapping();
	void updatey();
	void PSO();

	int OptimizerID;
	int OptimizerColorID;

    std::function<double(Eigen::Vector4f,bool,double )> costJ;
	Eigen::Vector4f variable;
	Eigen::Vector4f gradient;
	Eigen::Vector4f NeighbourVariable[4];
	Eigen::Vector4f NeighbourGradient[4];
	Eigen::Vector4f lastgradient;

	Eigen::Vector4f y;
	Eigen::Vector4f Neighbour_y[4];

	ros::Publisher variable_gradient_pub;
	ros::Subscriber variable_gradient_sub[3];
	int optimize_round{0};
	int selfgetflag[4];
	int neighbourgetflag[4];
	ros::Rate loop_rate;
	Eigen::Vector4f lasty;


	int othergetflag{0};
	int megetflag{0};
	bool GDInitFlag{false};

	Eigen::Matrix4f W;

	double current_v;

	Eigen::Vector4f particle[PARTICLE_NUM];
	Eigen::Vector4f particle_v[PARTICLE_NUM];
	std::pair<Eigen::Vector4f,double> localBestParticle[PARTICLE_NUM];
    std::pair<Eigen::Vector4f ,double> globalBestParticle;


	int round;


};


#endif